Robustify range finder kinematic consistency check #22117
Conversation
Even if there is some horizontal motion, a passing check should be accepted as the terrain can be flat. However, the vehicle must not be moving horizontally to invalidate the consistency as a change in terrain can make the kinematic check temporarily fail.
| // Run the kinematic consistency check when not moving horizontally | ||
| if (_control_status.flags.in_air && !_control_status.flags.fixed_wing | ||
| && (sq(_state.vel(0)) + sq(_state.vel(1)) < fmaxf(P.trace<2>(State::vel.idx), 0.1f))) { | ||
| if (_control_status.flags.in_air) { |
There was a problem hiding this comment.
What's the harm in letting this always run (!in_air)? The actual usage in the terrain estimator is still going to require in_air && rng_kin_consistent.
There was a problem hiding this comment.
An issue I see is when the operator is powering the drone but is still manipulating it before takeoff. There is then a high chance to fail the consistency check for no real reason. It should then be validated again after takeoff, so in theory it's not a big problem, the terrain estimator will just start a bit later and the 5s of no terrain aiding timeout should be enough.
| @@ -68,12 +72,20 @@ void RangeFinderConsistencyCheck::update(float dist_bottom, float dist_bottom_va | |||
|
|
|||
| void RangeFinderConsistencyCheck::updateConsistency(float vz, uint64_t time_us) | |||
| { | |||
| if (fabsf(vz) < _min_vz_for_valid_consistency) { | |||
| // We can only check consistency if there is vertical motion | |||
There was a problem hiding this comment.
Wouldn't you want to still invalidate if there's horizontal motion?
There was a problem hiding this comment.
No, we can validate regardless of the horizontal motion, but we can only invalidate if we're only moving vertically, otherwise a change in terrain height would invalidate too.
Solved Problem
I've seen logs where the kinematic consistency is false on takeoff and remains unchanged during the whole flight because the vehicle is always moving horizontally. The underlying kinematic consistency check was passing and it is highly unlikely to have a false passing detection because of terrain height changes (the terrain usually only creates false alarms).
Solution
Even if there is some horizontal motion, a passing check should be accepted as the terrain can be flat. However, the vehicle must not be moving horizontally to invalidate the consistency as a change in terrain can make the kinematic check temporarily fail.
Changelog Entry
For release notes:
Test coverage
replay on https://logs.px4.io/plot_app?log=466e8bd1-6100-42eb-a518-41b4cfbde149
Context
Related links, screenshot before/after, video